%% ��ջ���
clear
clc
close all

warning('off')
%% ��������
w = 0.6;      % �������� 
c1 = 2;       % ���ٳ���
c2 = 2;       % ���ٳ���

Dim = 10;            % ά��
SwarmSize = 20;    % ����Ⱥ��ģ
%ObjFun = @PSO_PID;  % ���Ż��������

MaxIter =50;      % ����������  
MinFit = 1e-12;       % ��С��Ӧֵ 

Vmax = 1;
Vmin = -1;
Ub = [0.44 80 0.5 29.6 23.1 80 0.49 29.6 23 15];
Lb = [-0.44 29 -0.236 0 9 29 -0.22 0 9 3.57];

%% ����Ⱥ��ʼ��
    Range = ones(SwarmSize,1)*(Ub-Lb);
    Swarm = rand(SwarmSize,Dim).*Range + ones(SwarmSize,1)*Lb ;     % ��ʼ������Ⱥ
    VStep = rand(SwarmSize,Dim)*(Vmax-Vmin) + Vmin      ;           % ��ʼ���ٶ�
    fSwarm = zeros(SwarmSize,1);
for i=1:SwarmSize
    fSwarm(i,:) = fun(Swarm(i,:));                         % ����Ⱥ����Ӧֵ
end

%% ���弫ֵ��Ⱥ�弫ֵ
[bestf bestindex]=min(fSwarm);
zbest=Swarm(bestindex,:);   % ȫ�����
gbest=Swarm;                % �������
fgbest=fSwarm;              % ���������Ӧֵ
fzbest=bestf;               % ȫ�������Ӧֵ

%% ����Ѱ��
iter = 0;
y_fitness = zeros(1,MaxIter);   % Ԥ�Ȳ���4���վ���
K_p = zeros(1,MaxIter);         
K_i = zeros(1,MaxIter);
K_d = zeros(1,MaxIter);
while( (iter < MaxIter) && (fzbest > MinFit) )
    iter
    for j=1:SwarmSize
        % �ٶȸ���
        VStep(j,:) = w*VStep(j,:) + c1*rand*(gbest(j,:) - Swarm(j,:)) + c2*rand*(zbest - Swarm(j,:));
        if VStep(j,:)>Vmax, VStep(j,:)=Vmax; end
        if VStep(j,:)<Vmin, VStep(j,:)=Vmin; end
        % λ�ø���
        Swarm(j,:)=Swarm(j,:)+VStep(j,:);
        for k=1:Dim
            if Swarm(j,k)>Ub(k), Swarm(j,k)=Ub(k); end
            if Swarm(j,k)<Lb(k), Swarm(j,k)=Lb(k); end
        end
        % ��Ӧֵ
        fSwarm(j,:) = fun(Swarm(i,:));
        % �������Ÿ���     
        if fSwarm(j) < fgbest(j)
            gbest(j,:) = Swarm(j,:);
            fgbest(j) = fSwarm(j);
        end
        % Ⱥ�����Ÿ���
        if fSwarm(j) < fzbest
            zbest = Swarm(j,:);
            fzbest = fSwarm(j);
        end
    end 
    iter = iter+1;                      % ������������
    y_fitness(1,iter) = fzbest;         % Ϊ��ͼ��׼��
    K_p(1,iter) = zbest(1);
    K_i(1,iter) = zbest(2);
    K_d(1,iter) = zbest(3);
end
%% ��ͼ���
figure(1)      % ��������ָ��ITAE�ı仯����
plot(y_fitness,'LineWidth',2)
title('���Ÿ�����Ӧֵ','fontsize',18);
xlabel('��������','fontsize',18);ylabel('��Ӧֵ','fontsize',18);
set(gca,'Fontsize',18);

figure(2)      % ����PID�����������仯����
subplot(3,1,1)
plot(K_p)
subplot(3,1,2)
plot(K_i,'k','LineWidth',3)
subplot(3,1,3)
plot(K_d,'--r')
% title('Kp��Ki��Kd �Ż�����','fontsize',18);
% xlabel('��������','fontsize',18);ylabel('����ֵ','fontsize',18);
% set(gca,'Fontsize',18);
% legend('Kp','Ki','Kd',1);

% sys=tf([87.49],[1 2100 3301000]);
% control=pid(K_p(end), K_i(end), K_d(end),0.02);
% figure(3)

% step(feedback(sys*control,1),'-',0.02);
% axis([0 0.02 0 1.2e-5])
assignin('base','Kp',K_p(end));
assignin('base','Ki',K_i(end));
assignin('base','Kd', K_d(end));
[t_time,x_state,y_out]=sim('PID_Model',[0,0.02]);
figure

plot(yy(:,1),yy(:,2))
